module CamReader(
	iCLK,
	iPCLK,
	iData,
	iX,
	iY,
	iFrame,
	iRST,
	oDebug,
	oStatus,
	//Acos side
	oTrigStart,
	oTrigFunc,
	iTrigBusy,
	iTrigError,
	oTrigData,
	iTrigResult,
	//UART
	oTXD,
	iTXStart
);

input iCLK;
input iPCLK;
input[7:0] iData;
input[9:0] iX, iY;
input iFrame;
input iRST;
output[178:0] oDebug;
output[14:0] oStatus;
//External acos
output reg oTrigStart;
output reg [1:0] oTrigFunc = 2'b10;
input iTrigBusy, iTrigError;
output reg [31:0] oTrigData;
input [31:0] iTrigResult;
output oTXD;
input iTXStart;


assign oStatus = {fifo_full, fifo_empty, fifo_rd, fifo_wr, busy, state, prev_state};
assign oDebug = { light_index, 5'd0, light_num, alfa, DivB, DivA, SinglePrecFloatSqrtA, 2'd0, w, y, x};

localparam CENTERX = 320;
localparam CENTERY = 240;
localparam PI2		 = 32'h40c90fdb;	// 2*Pi
localparam C03		 = 31'h3E99999A;	// 0.3

reg[1:0] frame;
always@(negedge iFrame or posedge iRST)begin
	if(iRST)
		frame <= 'd0;
	else 
		if(frame < 2'b11) frame <= frame + 1'd1;
end

reg[9:0] width, center;
reg[1:0] state_read;
reg[9:0] read_x, read_y, read_w;
always@ (posedge iPCLK or posedge iRST) begin
	if(iRST) begin
		fifo_rd <= 'b0;
		fifo_wr <= 'b0;
		state_read <= 'b0;
		width <= 'd0;
	end
	else begin
		if((iX == 0) &&(iY == 0)) begin
			fifo_in <= 30'd0;
			fifo_wr <= 'b1;
		end
		else begin
			if(iData > 'd220) begin
				if(width == 'd0) center <= iX;
				else if(width[0] == 'd0) center <= center + 1'd1;
				width <= width + 1'd1;
				fifo_wr <= 'b0;
			end
			else begin
				width <= 'd0;	
				if(width > 'd2)begin	
					fifo_in <= {center, iY, width};
					fifo_wr <= 'b1;
				end
				else fifo_wr <= 'b0;
			end
		end
		
		case(state_read)
			0: begin		// read fifo if not empty
				if(~fifo_empty)begin
					fifo_rd <= 'b1;
					state_read <= 'b1;
				end
			end		
			1: begin		// wait for data 
				fifo_rd <= 'b0;
				{read_x, read_y, read_w} <= fifo_out;
				start <= ~start;
				if(busy)	state_read <= 'd2;
			end		
			2: begin
				start <= 'b0;
				if(!busy) state_read <= 'b0;
			end
		endcase
	end	
end

reg start, prev_start, busy;
reg[4:0] state, timer, prev_state;
reg[9:0] x, y, w;
reg[31:0] alfa, radius, fwidth;
reg[6:0] light_num;
always@(posedge iCLK or posedge iRST) begin	
	if(iRST) begin
		busy <= 'd0;
		state <= 'd0;
		prev_start <= 1'b1;
		light_num <= 'd0;
	end
	else begin
		prev_start <= start;
		if(!busy) begin
			if({prev_start, start} == 'b01) begin
				busy  <= 'd1;
				if(read_w == 10'd0) state <= 'd31;	
				else state <= 'd0;								
				timer <= 'd0;
				x <= read_x;
				y <= read_y;
				w <= read_w;
				light_wr <= 'd0;
			end
		end
		else begin
			case(state)
				0: begin
					IntToFP1A <= (CENTERX - x)*(CENTERX - x) + (CENTERY - y)*(CENTERY - y);
					state <= 'd1;
					prev_state <= 'd0;
				end
				
				1: begin			
					if(timer == 'd6)begin
						IntToFP1A <= (CENTERY - y);
						SinglePrecFloatSqrtA <= IntToFP1Res;
						timer <= 'd0;
						state <= 'd3;
						prev_state <= 'd1;
					end
					else timer <= timer + 5'b1;
				end
				
				3:begin
					if(timer == 'd16)begin
						DivA <= IntToFP1Res;
						DivB <= SinglePrecFloatSqrtRes;
						radius <= SinglePrecFloatSqrtRes;
						IntToFP1A <= w;
						timer <= 'd0;
						state <= 'd2;
						prev_state <= 'd3;
					end
					else timer <= timer + 5'b1;					
				end
				
				2:begin					
					if(timer == 'd6) begin
						timer <= 'd0;
						state <=  'd6;
						oTrigData <= DivRes;
						oTrigStart <= 'b1;
						fwidth <= IntToFP1Res;
						prev_state <= 'd2;
					end
					else timer <= timer + 5'b1;			
				end
				
				6: begin
						prev_state <= 'd6;
						if(iTrigBusy == 'b1) state <=  'd4;
						else if(iTrigError == 'b1)state <= 'd8;		///ERROR
						else oTrigStart <= ~oTrigStart;
					end
				
				4:begin
					oTrigStart <= 'b0;
					prev_state <= 'd4;
					if(iTrigError == 'b1)state <= 'd8;		///ERROR
					else if(iTrigBusy == 'b0) begin
						if(x < 10'd321) begin
							alfa <= iTrigResult;
							state <= 'd7;
						end
						else begin
							Add1A <= PI2;
							Add1B <= {~iTrigResult[31],iTrigResult[30:0]};
							state <=  'd5;
						end
					end
				end
				
				5:begin			
					prev_state <= 'd5;
					if(timer == 'd7) begin
						timer <= 'd0;
						state <= 'd7;
						alfa <= Add1Res;
						light_index <= 0;
					end
					else timer <= timer + 1'b1;
				end
				
				7:begin
					prev_state <= 'd7;					
					if(light_index < light_num) begin
						Add1A <= light_out[31:0];
						Add1B <= {~alfa[31], alfa[30:0]};						
						state <= 5'd15;
					end
					else state <= 'd23;
				end
				
				15: begin
					prev_state <= 'd15;
					if(timer == 'd7) begin
						timer <= 'd0;
						if(Add1Res[30:0] < C03) state <= 'd14;
						else begin
							light_index <= light_index + 1'b1;
							state <= 'd7;
						end
					end
					else timer <= timer + 5'b1;
				end
				
				14: begin
					prev_state <= 'd14;
					if(timer == 'd1) begin
						timer <= 'd0;
						busy <= 'd0;
						light_wr <= 'd0;
						state <= 'd0;
					end
					else begin
						light_data <= {fwidth, radius, alfa};
						light_wr <= 'd1;
						timer <= timer + 5'b1;
					end				
				end
				
				23: begin			
					prev_state <= 'd23;
					if(timer == 'd1) begin
						timer <= 'd0;
						busy <= 'd0;
						light_wr <= 'd0;
						state <= 'd0;
					end
					else begin					
						light_data <= {fwidth, radius, alfa};
						light_wr <= 'd1;
						light_num <= light_num + 1'd1;
						timer <= timer + 5'b1;
					end
				end
				
				31: begin
					prev_state <= 'd31;
					if(timer == 'd1) begin
						timer <= 'd0;
						busy <= 'd0;
						state <= 'd0;
						light_num <= 'd0;
					end
					else timer <= timer + 5'b1;
				end
				//default:
				//	busy <= 'd0;
				
			endcase
		end
	end
end

wire uart_busy;
wire[17:0] uart_address;
wire[6:0] mod; 
assign mod = uart_address % 'd12;
wire[7:0] uart_data;
assign uart_data = (mod == 'd0 ? light_out[7:0] :
						 mod == 'd1 ? light_out[15:8] :
						 mod == 'd2 ? light_out[23:16] :
						 mod == 'd3 ? light_out[31:24] :
						 mod == 'd4 ? light_out[39:32] :
						 mod == 'd5 ? light_out[47:40] :
						 mod == 'd6 ? light_out[55:48] :
						 mod == 'd7 ? light_out[63:56] :
						 mod == 'd8 ? light_out[71:64] :
						 mod == 'd9 ? light_out[79:72] :
						 mod == 'd10 ? light_out[87:80] : light_out[95:88]);

UART_Controller uart(
	//Host side
	.iCLK(iCLK),
	.iStartTransmit(iTXStart),
	.iStartAddress(0),
	.iEndAddress(1535),
	.oBusy(uart_busy),
	//UART transmitter side
	.oTXD(oTXD),
	//MEmory side
	.oMEM_ADDR(uart_address),
	.iMEM_DATA(uart_data),
	.oMEM_CLK()
);

reg[29:0] fifo_in;
wire[29:0] fifo_out;
wire fifo_empty, fifo_full;
wire[7:0] fifo_usedw;
reg fifo_wr, fifo_rd;
FIFO fifo(
		.clock(~iPCLK),
		.data(fifo_in),
		.rdreq(fifo_rd),
		.sclr(iRST),
		.wrreq(fifo_wr),
		.empty(fifo_empty),
		.full(fifo_full),
		.q(fifo_out),
		.usedw(fifo_usedw)
);

reg[6:0] light_index;
reg[95:0] light_data;
reg light_wr;
wire[95:0] light_out;
LightRam lights(
	.address(uart_busy ? uart_address / 'd12: light_index),
	.clock(~iCLK),
	.data(light_data),
	.wren(light_wr),
	.q(light_out)
);

//6T
reg [31:0] IntToFP1A;
wire [31:0] IntToFP1Res;
IntToSinglePrecFloatConvert i2fp(
	.clock(~iCLK),
	.dataa(IntToFP1A),
	.result(IntToFP1Res)
);

//16T
reg [31:0] SinglePrecFloatSqrtA;
wire [31:0] SinglePrecFloatSqrtRes;
SinglePrecFloatSqrt sqrt(
	.clock(~iCLK),
	.data(SinglePrecFloatSqrtA),
	.result(SinglePrecFloatSqrtRes)
);

//6T
reg[31:0] DivA, DivB;
wire[31:0] DivRes;
SinglePrecFloatDiv div(
	.dataa(DivA),
	.datab(DivB),
	.result(DivRes),
	.clock(~iCLK)
);

//7T
reg[31:0] Add1A, Add1B;
wire[31:0] Add1Res;
SinglePrecFloatAdd add(
	.clock(~iCLK),
	.dataa(Add1A),
	.datab(Add1B),
	.result(Add1Res)
);

endmodule
